#pragma once
// #include "opencv2/imgproc.hpp"
// #include "opencv2/highgui.hpp"
// #include "opencv2/opencv.hpp"

// #include "ceres/ceres.h"

#include "Airplane.hpp"

#include "eigen3/Eigen/Dense"
#include <vector>
#include <iostream>
#include <math.h>
#include <string>

#include <ceres/ceres.h>
#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <glog/logging.h>
#include <glog/raw_logging.h>
#include <memory>

struct DistanceResidual {
    DistanceResidual(const Eigen::Vector3d& camera_position, const Eigen::Vector3d& unit_vector)
        : camera_position_(camera_position), unit_vector_(unit_vector) {}

    template <typename T>
    /**
     * @brief 计算目标位置到相机光线的最近点距离
     *
     * 根据给定的目标位置和相机位置，计算目标位置到相机光线的最近点距离，并将结果存储在残差中。
     *
     * @param target_position 目标位置指针
     * @param residual 残差数组指针
     *
     * @return 如果计算成功，则返回true；否则返回false
     */
    bool operator()(const T* const target_position, T* residual) const {
        Eigen::Matrix<T, 3, 1> T_target(target_position[0], target_position[1], target_position[2]);
        Eigen::Matrix<T, 3, 1> T_camera(camera_position_.cast<T>());
        Eigen::Matrix<T, 3, 1> T_unit(unit_vector_.cast<T>());

        Eigen::Matrix<T, 3, 1> target_to_camera = T_target - T_camera;
        T projection_length = target_to_camera.dot(T_unit);
        Eigen::Matrix<T, 3, 1> closest_point = T_camera + projection_length * T_unit;

        residual[0] = (T_target - closest_point).norm();
        return true;
    }

    const Eigen::Vector3d camera_position_;
    const Eigen::Vector3d unit_vector_;
};

struct DDXTJC_Camera
{
    float gamma_u;
    float gamma_v;
    float center_u;
    float center_v;
    int width;
    int height;
};

struct DDXTJC_Coordinate{
    Eigen::Matrix4d camera_matrix;
    Eigen::Matrix4d world_matrix;
    Eigen::Matrix4d camera_matrix_inv;
    Eigen::Matrix4d world_matrix_inv;
};



// S2 根据飞机的运算结果计算S3的初值X0
class DDXTJC
{
private:

    bool finished;
    // 位置状态
    Eigen::VectorXd target_position_;
    // 观测值
    Eigen::VectorXd Z;
    // time 
    float time;
    //std::vector<DDXTJC_Input> S2_Inputs_;

    /**
     * @brief 计算相机坐标系下的单位向量
     *
    */
    Eigen::Vector3d calculate_unit_vector(Gimbal_Camera camera, Picture_Location picture_loc);
    Eigen::Vector3d calculate_unit_vector(DDXTJC_Camera camera, int u, int v);
    std::vector<Eigen::Vector3d> calculate_unit_vectors(DDXTJC_Camera camera,Eigen::VectorXd Z);

    /**
     * @brief 计算世界坐标系下的观测轴单位向量
     *
     * 根据给定的相机坐标系下的向量、全局旋转角和无人机旋转角，计算世界坐标系下的观测轴单位向量。
     */
    Eigen::Vector3d calculateWorldViewingAxisUnitVector(const Eigen::Vector3d& v_C_i, Eigen::MatrixXd CoRotationMatrix);

    /**
     * @brief 计算世界坐标系下目标点的位置
     * 
     */                               
    Eigen::Vector3d calculatorActorPosition(std::vector<Eigen::Vector3d> CameraPosition,std::vector<Eigen::Vector3d> unitVectors);
    // Eigen::Vector3d calculateWorldViewingtargGetWorldPosition(Eigen::VectorXd Z, std::vector<Eigen::Vector3d> camera_position,DDXTJC_Camera camera);

public:
    DDXTJC(/* args */);
    ~DDXTJC();

   // void SetS2_Inputs_(std::vector<DDXTJC_Input> S2_Inputs);

    /**
     * @brief S2 步骤整体计算
     * 
     * 遍历所有飞机，输出目标位置和基准时间
     */ 
    bool update_Position(Eigen::Vector3d &TargetPosition, uint64_t &meanTime,
                         std::vector<DDXTJC_Input> Inputs);
                         
};

